package lego.Moving;

import lego.BT.BTController;
import lego.jbv.Display;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.IRSeekerV2;
import lejos.nxt.addon.IRSeekerV2.Mode;
import lejos.util.Delay;

public class Escape implements Behavior{
	private static IRSeekerV2 seeker;
	private boolean suppressed = false;
	private int direction = 0;
	private int sensorVal[] = new int[5];
	private final int minValueToTakeControl = 30;
	private final int delays = 300;
	private int max;
	
	private BTController btcontrol;
	
	public Escape(BTController btcontrol)
	{
		this.btcontrol = btcontrol;
		seeker = new IRSeekerV2(SensorPort.S2, Mode.DC);
	}
	
	@Override
	public int takeControl() {
		if(minValueToTakeControl < lookingForTrouble())
		{
			return 90;
		}
		
		return 0;
	}

	@Override
	public void action() {
		suppressed = false;
		Display.setStatus("ESCAPE " + max);

		int speed = Display.getBurstSpeed();
		
		if(Display.getStamina() < Display.getStaminaHighBound())
		{
			speed = Display.getNormalSpeed();
		}
		else
		{
			Display.setStamina(Display.getStamina()-1);
		}
		
		if(direction < 5 & direction > 0)
		{
			Car.drive(speed, 0);
			Delay.msDelay(delays);
			Car.stop();
		}
		else if(direction > 5)
		{
			Car.drive(0, speed);
			Delay.msDelay(delays);
			Car.stop();
		}
		else if(direction == 5)
		{
			Car.drive(speed, speed);
			Delay.msDelay(delays*2);
			Car.stop();
		}	
		if(max > 80)
		{
			Display.setStatus("Hit detected");
			btcontrol.SendHit();
		}
	}
	
	private int lookingForTrouble()
	{
		for(int i = 1; i <= 5; i++)
		{
			sensorVal[i-1] = seeker.getSensorValue(i);
		}
		
		max = 0;
		for(int i = 0; i < 5; i++)
		{
			if(sensorVal[i] > max)
				max = sensorVal[i];
		}
		
		direction = seeker.getDirection();
		
		return max;
	}

	@Override
	public void suppress() {
		suppressed = true;
	}
}